/*
控制机器人移动主要为两个命令:
- `set_twist(linear_x, linear_y, angular_z, duration)`：设置机器人速度和方向，`duration`为持续时间，单位为秒。
- `stance(duration)`：将机器人置于站立姿态，`duration`为持续时间，单位为秒。

当移动速度不为零时，机器人会自动从站立姿态转换到行走状态；当在行走状态下，速度为零时，机器人为原地踏步；
当在行走状态下，调用`stance()`函数时，机器人会从行走状态转换到站立姿态。

站立切换到行走状态时，机器人会等待500毫秒以确保稳定性。
从行走状态切换到站立状态时，机器人会等待至少2秒以确保稳定性。

用例请参考: src/navigation/src/demo_twist.cpp
启动方法参考: README.md#一个cmd_vel控制移动的demo
*/
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <navigation/cmd_publisher/joy_publisher.hpp>
#include <thread>
#include <mutex>
#include <atomic>

class TwistPublisher {
public:
    explicit TwistPublisher(const rclcpp::Node::SharedPtr& node, double hz = 10.0)
    : node_(node), hz_(hz), running_(true), pause_(true) {
        twist_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
        current_twist_ = geometry_msgs::msg::Twist();
        thread_ = std::thread(&TwistPublisher::publish_loop, this);
        joy_publisher_ = std::make_unique<JoyPublisher>(node, hz);
    }
    ~TwistPublisher() {
        running_ = false;
        if (thread_.joinable()) thread_.join();
    }
    void set_twist(double linear_x, double linear_y, double angular_z, double duration = 0.0) {
        geometry_msgs::msg::Twist twist;
        twist.linear.x = linear_x;
        twist.linear.y = linear_y;
        twist.angular.z = angular_z;
        if (linear_x != 0.0 || linear_y != 0.0 || angular_z != 0.0) {
            walk();  // Ensure the robot is in walk mode if moving
        }
        set_twist(twist);
        if (duration > 0.0) {
            rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int64_t>(duration * 1e3)));
            stance();  // Stop after the duration
        }
    }
    void stance(double duration = 2.0) {
        if (!is_stance_) {
            joy_publisher_->publish_stance(1.5, 0.5);
            is_stance_ = true;
            RCLCPP_INFO(node_->get_logger(), "Transitioning from walk to stance.");
            duration = std::max(duration, 3.0);  // Ensure stance lasts at least 4 second
        }
        set_twist(0.0, 0.0, 0.0);  // Ensure the robot vel is zero in stance
        if (duration > 0.0) {
            rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int64_t>(duration * 1e3)));
        }
        pause_ = true;  // If stance pause seend
    }
    void walk(double duration = 0.0) {
        if (is_stance_) {
            joy_publisher_->publish_walk();
            is_stance_ = false;
            RCLCPP_INFO(node_->get_logger(), "Transitioning from stance to walk.");
            duration = std::max(duration, 0.5);  // Ensure walk lasts at least 0.5 second
        }
        if (duration > 0.0) {
            rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int64_t>(duration * 1e3)));
        }
        pause_ = false;
    }
private:
    void set_twist(const geometry_msgs::msg::Twist& twist) {
        std::lock_guard<std::mutex> lock(mutex_);
        current_twist_ = twist;
    }
    void publish_loop() {
        auto period = std::chrono::duration<double>(1.0 / hz_);
        while (running_) {
            if (!pause_) {
                geometry_msgs::msg::Twist twist_copy;
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    twist_copy = current_twist_;
                }
                twist_pub_->publish(twist_copy);
            }
            rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(period));
        }
    }
    rclcpp::Node::SharedPtr node_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
    double hz_;
    std::thread thread_;
    std::mutex mutex_;
    geometry_msgs::msg::Twist current_twist_;
    std::atomic<bool> running_;
    std::atomic<bool> pause_;
    std::unique_ptr<JoyPublisher> joy_publisher_;
    bool is_stance_ = true;
};
